#include "slamwidget.h"
#include <QProcess>
#include <QDebug>
#include <QDateTime>
#include <QHBoxLayout>
#include <QVBoxLayout>
#include <QLabel>
#include <QFileDialog>
#include <pcl_conversions/pcl_conversions.h>
#include "util.h"
#include "node/rosnode.h"
#include "util/topicfiled.h"
#include "tools/leiliconfig.h"

const unsigned char openCmd[]={0xA0,0x01,0x01,0xA2};
const unsigned char closeCmd[]={0xA0,0x01,0x00,0xA1};
const unsigned char queryCmd[]={0xA0,0x01,0x02,0xA3};

SLAMwidget::SLAMwidget(QWidget *parent) : QWidget(parent)
{
    cmdList = new QStringList();

    const char* rosDistro = std::getenv("ROS_DISTRO");
    const char* rosVersion = std::getenv("ROS_VERSION");

    if(rosVersion)
    {
        m_rosVersion = std::atoi(rosVersion);
    }

    qInfo() << "ROS_VERSION:" << m_rosVersion << " rosDistro:" << rosDistro;

    if(m_rosVersion == 1){
        cmdList->push_back(" roslaunch i2ekf_lo velodyne.launch");
    }
    else{
        cmdList->push_back("ros2 launch lego_loam_sr run.launch.py");
        cmdList->push_back(" ros2 launch octomap_server2 octomap_server_launch.py");
        cmdList->push_back(" ros2 run nav2_map_server map_saver_server");
        cmdList->push_back(" ros2 lifecycle set /map_saver configure");
        cmdList->push_back(" ros2 lifecycle set /map_saver activate");
    }

    initUI();

    m_serial = new QSerialPort(this);

    createConn();

}

SLAMwidget::~SLAMwidget()
{
    delete  cmdList;

    onStopSLAM();
    onStopRviz();
}

void SLAMwidget::onStartSLAM()
{
    foreach(const QString &cmd,*cmdList) {

        QStringList cmds = cmd.split(' ');
        if(cmds.empty())
        {
            continue;
        }

        QProcess *process = new QProcess(this);

        connect(process, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished), [process](int exitCode, QProcess::ExitStatus exitStatus) {
               qDebug() << process->program() << process->arguments()<<process->pid() <<" Node exited with code" << exitCode << process->errorString();

              // process->deleteLater(); // 清理资源
           });
        QString fistCmd =cmd;

        process->start(fistCmd);

        if (!process->waitForStarted())
        {
            qWarning() << fistCmd << " " <<cmds << " start failed." << process->errorString();
            process->deleteLater();
        }else{
            qDebug() << fistCmd << " " <<cmds  << process->processId()<< "succ";
            m_processes.push_back(process);
            pidList.push_back(process->processId());
        }
    }

    m_rviz->DemoveAllDisplays();
    m_rviz->Display_LeGoLoAMProject();
}


void SLAMwidget::onPauseSLAM()
{

}

void SLAMwidget::onStopSLAM()
{
    saveMap();
    QVector<quint64> vecChildId;
    for(int i =0; i < pidList.size(); i++)
    {
       get_child_pids(pidList[i],vecChildId);
    }


    foreach(auto pid, vecChildId)
    {
        qDebug() << "try to kill pid" << pid;
        kill_process(pid);
    }

    foreach(QProcess *process, m_processes) {
        if (process) {
            if (!(process->state() == QProcess::NotRunning)) {

                process->terminate();
                process->waitForFinished(1000);
            }
            process->deleteLater();
            process = NULL;
        }
    }

    m_processes.clear();
}

void SLAMwidget::onSaveSLAM()
{

}

void SLAMwidget::onStartPathPlaning()
{
    QProcess *process = new QProcess(this);
    connect(process, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished), [process](int exitCode, QProcess::ExitStatus exitStatus) {
           qDebug() << "Node exited with code" << exitCode;
          // process->deleteLater(); // 清理资源
       });
    QString cmd = "docker exec -it melodic1 /bin/bash ";

    process->startDetached(cmd);

    if (!process->waitForStarted())
    {
        qWarning() << cmd << " start failed." << process->errorString();
        process->deleteLater();
    }else{
        process->write(" roslaunch MSTC_Star_update main.launch\r\n");

        process->waitForBytesWritten();
        m_processes.push_back(process);
        qDebug() << cmd << "succ";
    }
}

void SLAMwidget::initUI()
{
    setContentsMargins(0,0,0,0);
    QVBoxLayout *p = new QVBoxLayout(this);
    p->setSpacing(0);
    p->setContentsMargins(0,0,0,0);

    QHBoxLayout *hL = new QHBoxLayout(this);

    m_btnOpenLader = new QPushButton(this);
    m_btnOpenLader->setText("打开雷达");
    m_btnOpenLader->setObjectName("closeLadar");
    m_btnOpenLader->setCursor(QCursor(Qt::PointingHandCursor));

    m_btnShowLader = new QPushButton(this);
    m_btnShowLader->setText("显示雷达");
    m_btnShowLader->setObjectName("laderHide");
    m_btnShowLader->setCursor(QCursor(Qt::PointingHandCursor));

    m_btnSLAM = new QPushButton(this);
    m_btnSLAM->setText("开始建图");
    m_btnSLAM->setObjectName("startSLAM");
    m_btnSLAM->setStyle(m_btnSLAM->style());
    m_btnSLAM->setCursor(QCursor(Qt::PointingHandCursor));

    m_btnOpenRviz = new QPushButton(this);
    m_btnOpenRviz->setText("打开rviz");
    m_btnOpenRviz->setObjectName("openRviz");
    m_btnOpenRviz->setCursor(QCursor(Qt::PointingHandCursor));

    m_btnOpenPcd = new QPushButton(this);
    m_btnOpenPcd->setText("查看建图结果");
    m_btnOpenPcd->setCursor(QCursor(Qt::PointingHandCursor));


    hL->addStretch(1);
    hL->addWidget(m_btnOpenLader);
    hL->addSpacing(12);
    hL->addWidget(m_btnShowLader);
    hL->addSpacing(48);
    hL->addWidget(m_btnSLAM);
    hL->addSpacing(48);
    hL->addWidget(m_btnOpenRviz);
    hL->addWidget(m_btnOpenPcd);

    hL->addStretch(1);



    m_rviz = new QRviz(this);

    m_rviz->setObjectName("rviz");
    m_rviz->setStyle(m_rviz->style());
    //m_rviz->Display_LeGoLoAMProject();
    //m_rviz->Load("/home/zhangsan/catkin_ws/src/I2EKF-LO/rviz_cfg/spinning.rviz");
    //todo解析配置文件逻辑
    //m_rviz->Load("./test.rviz");

    m_rviz->DisPlay_Point2(true, QString::fromStdString(TopicFiled::pcd2point2));

    m_ladarRviz = new QRviz(this);
    m_ladarRviz->setObjectName("rvizLadar");
    m_ladarRviz->setFixedSize(400,400);
    m_ladarRviz->Display_Lader();
    //m_ladarRviz->hide();

    this->setStyle(this->style());

    p->addWidget(m_rviz) ;
    p->addLayout(hL);
    p->addSpacing(6);


    //onInitRviz();
    initialVtkWidget();
}

void SLAMwidget::createConn()
{
     QObject::connect(m_btnSLAM,&QPushButton::clicked,this,&SLAMwidget::onSLAMClick);
     QObject::connect(m_btnShowLader,&QPushButton::clicked,this,&SLAMwidget::onShowLadarImg);
     QObject::connect(m_btnOpenRviz,&QPushButton::clicked,this,&SLAMwidget::onRvizClick);
     QObject::connect(m_btnOpenPcd,&QPushButton::clicked,this,&SLAMwidget::onOpenPCD);

}

void SLAMwidget::exitProcess()
{

}

void SLAMwidget::saveMap()
{
    QProcess *process = new QProcess(this);

    connect(process, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished), [process](int exitCode, QProcess::ExitStatus exitStatus) {
           qDebug() << "Node exited with code" << exitCode;
           process->deleteLater(); // 清理资源
       });

    qint64 ts = QDateTime::currentSecsSinceEpoch();
    m_mapFullName = "/opt/leili/map/" + QString::number(ts);
    QString cmd =  QString::asprintf("ros2 service call /map_saver/save_map nav2_msgs/srv/SaveMap \
                    {map_topic: '/projected_map', \
                    map_url: '%1', \
                    image_format: 'pgm', \
                    map_mode: 'trinary', \
                    free_thresh: 0.25, \
                    occupied_thresh: 0.65}\"").arg(m_mapFullName);
    process->start(cmd);
    if (!process->waitForStarted())
    {
        qWarning() << cmd << " start failed." << process->errorString();
        process->deleteLater();
    }else{
        qDebug() << cmd << "succ";
    }
}

void SLAMwidget::onSLAMClick()
{
    if(isSLAMing)
    {
        onStopSLAM();
    }
    else
    {
        onStartSLAM();
    }
    isSLAMing = !isSLAMing;

    if(isSLAMing)
    {
        m_btnSLAM->setText("停止建图");
        m_btnSLAM->setObjectName("stopSLAM");
    }
    else
    {
        m_btnSLAM->setText("重新建图");
        m_btnSLAM->setObjectName("restartSLAM");
    }

    m_btnSLAM->setStyle(m_btnSLAM->style());
}

void SLAMwidget::onShowLadarImg()
{
    if(!m_laderRvizState)
    {
       m_ladarRviz->Show();
       m_ladarRviz->show();
       m_btnShowLader->setText("隐藏雷达");
       m_btnShowLader->setObjectName("laderShow");
    }
    else
    {
        m_ladarRviz->hide();
        m_ladarRviz->Hide();
        m_btnShowLader->setText("显示雷达");
        m_btnShowLader->setObjectName("laderHide");
    }

    m_laderRvizState = !m_laderRvizState;
    m_btnShowLader->setStyle(m_btnShowLader->style());
}

void SLAMwidget::onOpenLadar()
{
    if(!m_serial->isOpen())
    {
        //tips->setText("请先连接串口");
    }

    if(m_btnOpenLader->text() == "关闭雷达")
    {

        m_serial->write((char*)closeCmd,sizeof(closeCmd));
        return;
    }

    if(m_btnOpenLader->text() == "打开雷达")
    {

        m_serial->write((char*)openCmd,sizeof(openCmd));
        return;
    }


    m_serial->write((char*)queryCmd, sizeof(queryCmd));
}

void SLAMwidget::onSerialError(QSerialPort::SerialPortError e)
{
    switch (e) {

    case QSerialPort::OpenError:
        emit statusBarMessage(QtWarningMsg, "打开设备时出错");
        break;

    case QSerialPort::DeviceNotFoundError:
        emit statusBarMessage(QtWarningMsg, "找不到设备");
        break;
    case QSerialPort::PermissionError:
        emit statusBarMessage(QtWarningMsg, "没有足够的权限,请检查设备连接状态");

        break;
    case QSerialPort::ResourceError:
        emit statusBarMessage(QtWarningMsg, "设备被意外移除或系统资源不足");
        break;
    case QSerialPort::WriteError:
        emit statusBarMessage(QtWarningMsg, "写数据时出错");
        break;
    case QSerialPort::ReadError:
        emit statusBarMessage(QtWarningMsg, "读数据时出错");
        break;

    default:
        if(e !=QSerialPort::NoError){
            emit statusBarMessage(QtWarningMsg, "串口错误，请检查设备");
        }
    }


    if(e !=QSerialPort::NoError)
    {
        if(m_serial->isOpen())
        {
            m_serial->close();
        }

        //comboBox->setEnabled(true);
        //btnSerialConnect->setText("打开串口");
        //updateComPort();
    }
}

void SLAMwidget::onRvizClick()
{
    if(!m_rvizPid){
        onOpenRviz();
        m_btnOpenRviz->setText("关闭rviz");
    }else{
        onStopRviz();
        m_btnOpenRviz->setText("打开rviz");
    }
}

void SLAMwidget::onOpenRviz()
{
    QProcess *process = new QProcess(this);

    connect(process, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished), [process](int exitCode, QProcess::ExitStatus exitStatus) {
           qDebug() << process->program() << process->arguments()<<process->pid() <<" Node exited with code" << exitCode << process->errorString();
          qDebug() << "slam rviz" << " " << process->processId()<< "exit";
          SLAMwidget *p = qobject_cast<SLAMwidget*>(process->parent());
          if(p)
          {
            p->onExitRviz();
          }
          process->deleteLater(); // 清理资源
       });

    QString cmd ="roslaunch i2ekf_lo runRviz.launch ";

    process->start(cmd);

    if (!process->waitForStarted()){
        qWarning() << cmd << "  start failed." << process->errorString();
        process->deleteLater();
    }else{
        qDebug() << cmd << " " << process->processId()<< "succ";
        m_rvizPid =process->processId();
        //m_rvizpidList.push_back(process->processId());
    }
}

void SLAMwidget::onStopRviz()
{
    if(m_rvizPid){
        kill_process(m_rvizPid);
    }
}

void SLAMwidget::onExitRviz()
{
    m_rvizPid  =0;
    m_btnOpenRviz->setText("打开rviz");
}

void SLAMwidget::onInitRviz()
{

}

void SLAMwidget::initialVtkWidget(){
    openGLWidget = new QVTKOpenGLNativeWidget();
    vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New();
    vtkSmartPointer<vtkGenericOpenGLRenderWindow> renderWindow =
        vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
    renderWindow->AddRenderer(renderer);
    view.reset(new pcl::visualization::PCLVisualizer(renderer, renderWindow,
                                                     "viewer", false));
    view->setupInteractor(openGLWidget->interactor(),
                          openGLWidget->renderWindow());
    openGLWidget->setRenderWindow(view->getRenderWindow());
}


void SLAMwidget::onOpenPCD(){
    QString fileName = QFileDialog::getOpenFileName(this, "Open PointCloud", LeiliCfg->getSavePath(),
                                                    "Open PCD files(*.pcd)",nullptr,QFileDialog::DontUseNativeDialog);
    if (fileName == "") return;


    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::io::loadPCDFile(fileName.toStdString(), *cloud);

    sensor_msgs::PointCloud2 pc2;

    pcl::toROSMsg (*cloud, pc2);

    pc2.header.frame_id = "map";
    GRosNode->pubPoint2(pc2);

    view->addPointCloud(cloud, "cloud");
    view->resetCamera();
    view->spin();
    openGLWidget->update();
    openGLWidget->show();
}
